#include "OOBB3Collision.h"
#include "Vec3.h"

//float Dot(const Vec3f& v1, const Vec3f& v2)
//{
//	return float(v1.x * v2.x + v1.y * v2.y + v1.z * v2.z);
//}

//Vec3f CrossProduct(const Vec3f& u, const Vec3f& v)
//{
//	Vec3f result(0, 0, 0);
//	result.x = u.y * v.z - u.z * v.y;
//	result.y = u.z * v.x - u.x * v.z;
//	result.z = u.x * v.y - u.y * v.x;
//
//	return result;
//}

bool CheckCollisionOptimised(const OOBB3& oobb1, const OOBB3& oobb2)
{
	BoxPoints3 bp1, bp2;

	CalculateOOBBCorners(oobb1, bp1);
	CalculateOOBBCorners(oobb2, bp2);

	float min1, min2, max1, max2;
	float diff = 0;
	// check the oobb's axes (6)

	for (int i = 0; i != 3; i++)
	{
		float dp1 = D3DXVec3Dot(&oobb1.axis[i], &bp1.corners[0]);
		min1 = dp1;
		max1 = dp1;
		float dp2 = D3DXVec3Dot(&oobb1.axis[i], &bp2.corners[0]);
		min2 = dp2;
		max2 = dp2;
		for (int corners = 0; corners != 8; corners++)
		{
			float  projectedCornerForBox1 = D3DXVec3Dot(&bp1.corners[corners], &oobb1.axis[i]);
			if (projectedCornerForBox1 > max1)
			{
				max1 = projectedCornerForBox1;
			}
			if (projectedCornerForBox1 < min1)
			{
				min1 = projectedCornerForBox1;
			}

			float  projectedCornerForBox2 = D3DXVec3Dot(&bp2.corners[corners], &oobb1.axis[i]);
			if (projectedCornerForBox2 > max2)
			{
				max2 = projectedCornerForBox2;
			}
			if (projectedCornerForBox2 < min2)
			{
				min2 = projectedCornerForBox2;
			}
		}
		if (!(min1 <= max2 && min2 <= max1))
		{
			return false;
		}

		dp1 = D3DXVec3Dot(&oobb2.axis[i], &bp1.corners[0]);
		min1 = dp1;
		max1 = dp1;
		dp2 = D3DXVec3Dot(&oobb2.axis[i], &bp2.corners[0]);
		min2 = dp2;
		max2 = dp2;

		for (int corners = 0; corners != 8; corners++)
		{
			float  projectedCornerForBox1 = D3DXVec3Dot(&bp1.corners[corners], &oobb2.axis[i]);
			if (projectedCornerForBox1 > max1)
			{
				max1 = projectedCornerForBox1;
			}
			if (projectedCornerForBox1 < min1)
			{
				min1 = projectedCornerForBox1;
			}

			float  projectedCornerForBox2 = D3DXVec3Dot(&bp2.corners[corners], &oobb2.axis[i]);
			if (projectedCornerForBox2 > max2)
			{
				max2 = projectedCornerForBox2;
			}
			if (projectedCornerForBox2 < min2)
			{
				min2 = projectedCornerForBox2;
			}
		}
		if (!(min1 <= max2 && min2 <= max1))
		{
			return false;
		}
	}

	// check the cross products (9)
	for (int obb1 = 0; obb1 != 3; obb1++)
	{
		for (int obb2 = 0; obb2 != 3; obb2++)
		{
			D3DXVECTOR3 cp;
			D3DXVec3Cross(&cp, &oobb1.axis[obb1], &oobb2.axis[obb2]);

			float dp1 = D3DXVec3Dot(&cp, &bp1.corners[0]);
			min1 = dp1;
			max1 = dp1;
			float dp2 = D3DXVec3Dot(&cp, &bp2.corners[0]);
			min2 = dp2;
			max2 = dp2;

			for (int corners = 0; corners != 8; corners++)
			{
				float  projectedCornerForBox1 = D3DXVec3Dot(&bp1.corners[corners], &cp);
				if (projectedCornerForBox1 > max1)
				{
					max1 = projectedCornerForBox1;
				}
				if (projectedCornerForBox1 < min1)
				{
					min1 = projectedCornerForBox1;
				}

				float  projectedCornerForBox2 = D3DXVec3Dot(&bp2.corners[corners], &cp);
				if (projectedCornerForBox2 > max2)
				{
					max2 = projectedCornerForBox2;
				}
				if (projectedCornerForBox2 < min2)
				{
					min2 = projectedCornerForBox2;
				}
			}
			if (!(min1 <= max2 && min2 <= max1))
			{
				return false;
			}
		}
	}



	return true;
}